#include<iostream>
#include"func.h"
#include<opencv2/imgcodecs.hpp>
#include<opencv2/highgui.hpp>
#include<opencv2/imgproc.hpp>
#include<opencv2/objdetect.hpp>
#include<opencv2/opencv.hpp>
#include<opencv2/ximgproc.hpp>
#include<string>
#include<vector>
#include<fstream>

using namespace std;
using namespace cv;
using namespace cv::ml;

void testf4()
{
    cout<<"func4 ready"<<endl;
}

void danmubiaoding()
{
    vector<Mat> imgs;
    string imgName;
    ifstream fin("/home/zhanjiabin/桌面/task/study/cal.txt");
    while(getline(fin,imgName))
    {
        Mat img=imread(imgName);
        imgs.push_back(img);
    }

    Size board_size=Size(7,9);//方格标定板内角点数目的(行，列)
    vector<vector<Point2f>> imgsPoints;
    for(int i=0;i<imgs.size();i++)
    {
        Mat img1=imgs[i];
        Mat gray1;
        cvtColor(img1,gray1,COLOR_BGR2GRAY);
        vector<Point2f> img1_points;
        findChessboardCorners(gray1,board_size,img1_points);//标定板内角点检测函数(输入图，内角点行列数，检测结结果坐标)
        find4QuadCornerSubpix(gray1,img1_points,Size(5,5));//坐标细化函数(输入图，检测到坐标，优化时考虑的邻域范围)
        bool patten=true;
        drawChessboardCorners(img1,board_size,img1_points,patten);//绘制方格标定板角点函数(目标图，内角点行列数，角点坐标，绘制角点样式标志)
        imshow("img1",img1);
        waitKey(0);
        imgsPoints.push_back(img1_points);
    }
    
    //生成棋盘内每个角点的空间三维坐标
    Size squareSize=Size(10,10);//棋盘内每个方格的真实尺寸
    vector<vector<Point3f>> objectPoints;
    for(int i=0;i<imgsPoints.size();i++)
    {
        vector<Point3f> tempPointSet;
        for(int j=0;j<board_size.height;j++)
        {
            for(int k=0;k<board_size.width;k++)
            {
                Point3f realPoint;
                //假设标定板为世界坐标的z平面，即z=0
                realPoint.x=j*squareSize.width;
                realPoint.y=k*squareSize.height;
                realPoint.z=0;
                tempPointSet.push_back(realPoint);
            }
        }
        objectPoints.push_back(tempPointSet);
    }

    /*初始化每幅图像中的角点数量，假定每幅图像中都可以看到完整的标定板*/
    vector<int> point_number;
    for(int i=0;i<imgsPoints.size();i++)
    {
        point_number.push_back(board_size.width*board_size.height);
    }

    //图像尺寸
    Size imgSize;
    imgSize.width=imgs[0].cols;
    imgSize.height=imgs[0].rows;

    Mat camMaxtrix=Mat(3,3,CV_32FC1,Scalar::all(0));//相机内参矩阵
    Mat distCorffs=Mat(1,5,CV_32FC1,Scalar::all(0));//相机五个畸变系数
    vector<Mat> rvecs;//每幅图片的旋转向量
    vector<Mat> tvecs;//每张图的平移量
    calibrateCamera(objectPoints,imgsPoints,imgSize,camMaxtrix,distCorffs,rvecs,tvecs,0);//相机标定函数(棋盘内点三维坐标，～二维坐标，图像像素尺寸大小，内参矩阵，畸变系数矩阵，相机坐标系与世界坐标系的旋转向量，～平移向量，标定算法标志)
    cout<<"内参矩阵"<<endl<<camMaxtrix<<endl;
    cout<<"畸变系数"<<endl<<distCorffs<<endl;//k1,k2,p1,p2,k3
    waitKey(0);

}

void lianpai()
{
    int num;
    int pai=0;
    VideoCapture cap(2);
    Mat imggot,imgshow;

    cout<<"输入连拍数量"<<endl;
    cin>>num;

    namedWindow("向右滑动开始连拍");
    createTrackbar("滑动条","向右滑动开始连拍",&pai,100);

    while(1)
    {
        cap.read(imgshow);
        imshow("相机画面",imgshow);
        if(pai>50)
        {
            for(int i=1;i<=num;i++)
            {
                cap.read(imggot);
                imwrite("/home/zhanjiabin/桌面/task/study/imggot"+to_string(i)+".jpg",imggot);
                imshow("imggot"+to_string(i),imggot);
                waitKey(1000);
            }
            // break;
        }
        else
        {
            waitKey(1);
        }
    }

    

}

void undist(vector<Mat> imgs,//原图相
            Mat camMatrix,   //内参矩阵
            Mat distCoeffs,  //畸变系数矩阵
            vector<Mat> &undistImgs)//矫正后输出图像
{
    for(int i=0;i<imgs.size();i++)
    {
        Mat undistImg;
        undistort(imgs[i],undistImg,camMatrix,distCoeffs);//去畸变函数(原图，输出图，内参矩阵，畸变矩阵)
        undistImgs.push_back(undistImg);
    }
}

void tuxiangjiaozheng()
{

    //读取所有图片
    vector<Mat> imgs;
    string imgName;
    ifstream fin("/home/zhanjiabin/桌面/task/study/cal.txt");
    while(getline(fin,imgName))
    {
        Mat img=imread(imgName);
        imgs.push_back(img);
    }

    //输入标定得到内参矩阵
    Mat camMatirx=(Mat_<float>(3,3)<<795.0127863242187, 0, 322.3527060362668,
                                        0, 677.1229723210331, 406.042367426904,
                                        0, 0, 1);
    //输入标定得到畸变系数矩阵
    Mat distCoffs=(Mat_<float>(1,5)<<-0.3291539046569694, -0.4888867792032087, -0.0887661873901151, 0.004208712804125788, 1.667786654640073);

    vector<Mat> undistimgs;

    undist(imgs,camMatirx,distCoffs,undistimgs);

    //显示校正结果
    for(int i=0;i<imgs.size();i++)
    {
        string winnum=to_string(i);
        imshow("未校正图像"+winnum,imgs[i]);
        imshow("校正后图像"+winnum,undistimgs[i]);
        waitKey(0);
        destroyWindow("未校正图像"+winnum);
        destroyWindow("校正后图像"+winnum);
    }

}

void shishiduibi()
{
    VideoCapture cap(2);//右侧usb口相机
    Mat img,imgundist;//原图，去畸变后图

    //输入标定得到内参矩阵
    Mat camMatirx=(Mat_<float>(3,3)<<1802.907160827488, 0, 334.5438233425143,
                                        0, 1540.82420238054, 282.9430555384519,
                                        0, 0, 1);
    //输入标定得到畸变系数矩阵
    Mat distCoeffs=(Mat_<float>(1,5)<<-0.3982176123731064, -67.04810226782725, -0.02403403419568416, -0.0002364398407027878, 1125.027744731815);

    while (1)
    {
        cap.read(img);
        imshow("img",img);//显示原图

        undistort(img,imgundist,camMatirx,distCoeffs);//去畸变函数(原图，输出图，内参矩阵，畸变矩阵)

        imshow("imgundist",imgundist);//显示去畸变后图

        waitKey(1);

    }
    
}

void danmuweizi()
{
    Mat img=imread("/home/zhanjiabin/桌面/task/study/imggot1.jpg");
    Mat gray;
    cvtColor(img,gray,COLOR_BGR2GRAY);
    vector<Point2f> imgPoints;
    Size boardSize=Size(7,9);
    findChessboardCorners(gray,boardSize,imgPoints);
    find4QuadCornerSubpix(gray,imgPoints,Size(5,5));//细化角点坐标

    //生成三维坐标
    Size squareSize=Size(10,10);//每个方格真实尺寸
    vector<Point3f> PointSets;
    for(int j=0;j<boardSize.height;j++)
    {
        for(int k=0;k<boardSize.width;k++)
        {
            Point3f realPoint;
            realPoint.x=j*squareSize.width;
            realPoint.y=k*squareSize.height;
            realPoint.z=0;
            PointSets.push_back(realPoint);
        }
    }

    //输入标定得到内参矩阵
    Mat camMatirx=(Mat_<float>(3,3)<<1802.907160827488, 0, 334.5438233425143,
                                        0, 1540.82420238054, 282.9430555384519,
                                        0, 0, 1);
    //输入标定得到畸变系数矩阵
    Mat distCoeffs=(Mat_<float>(1,5)<<-0.3982176123731064, -67.04810226782725, -0.02403403419568416, -0.0002364398407027878, 1125.027744731815);

    //用pnp算法计算旋转和平移量
    Mat rvec,tvec;
    solvePnP(PointSets,imgPoints,camMatirx,distCoeffs,rvec,tvec);//PNP解算函数(3D点坐标，内角点像素坐标，内参矩阵，畸变系数矩阵，旋转向量，平移向量)
    cout<<"旋转向量: "<<rvec<<endl;
    //旋转向量转旋转矩阵
    Mat R;
    Rodrigues(rvec,R);
    cout<<"旋转矩阵"<<endl<<R<<endl;

    //PNP+Ransac计算旋转和平移向量
    Mat rvecRansac,tvecRansac;
    solvePnPRansac(PointSets,imgPoints,camMatirx,distCoeffs,rvecRansac,tvecRansac);//PNP+Ransac解算函数(3D点坐标，内角点像素坐标，内参矩阵，畸变系数矩阵，旋转向量，平移向量)
    Mat RRansac;
    Rodrigues(rvecRansac,RRansac);
    cout<<"旋转矩阵2: "<<endl<<RRansac<<endl;
    waitKey(0);

}

void chazhigenzong()
{
    VideoCapture cap(2);
    int fps=cap.get(CAP_PROP_FPS);
    int width=cap.get(CAP_PROP_FRAME_WIDTH);
    int height=cap.get(CAP_PROP_FRAME_HEIGHT);
    int num_of_frames=cap.get(CAP_PROP_FRAME_COUNT);
    cout<<"视频宽度"<<width<<"视频高度"<<height<<"帧率"<<fps<<endl;

    //获取第一帧进行灰度化
    Mat preframe,pregray;
    cap.read(preframe);
    cvtColor(preframe,pregray,COLOR_BGR2GRAY);
    //高斯滤波减少噪声干扰
    GaussianBlur(pregray,pregray,Size(0,0),15);

    Mat binary;
    Mat frame,gray;
    //形态学操作模板
    Mat k=getStructuringElement(MORPH_RECT,Size(7,7),Point(-1,-1));

    while(1)
    {
        cap.read(frame);

        //灰度化当前帧
        cvtColor(frame,gray,COLOR_BGR2GRAY);
        GaussianBlur(gray,gray,Size(0,0),15);

        //计算当前帧和第一帧差值的绝对值
        absdiff(gray,pregray,binary);//图像差值计算函数(被减图，减图，商图)

        //对计算结果二值化并开运算减小噪声干扰
        threshold(binary,binary,10,255,THRESH_BINARY|THRESH_OTSU);
        morphologyEx(binary,binary,MORPH_OPEN,k);

        //显示处理结果
        imshow("input",frame);
        imshow("output",binary);

        //将当前帧变为前帧
        gray.copyTo(pregray);

        //5毫秒延迟判断是否退出程序
        char c=waitKey(5);
        if(c==27)
        {
            break;
        }

    }

    waitKey(0);
    
}

void choumigenzong()
{
    VideoCapture cap(2);
    Mat prevFrame,prevgGray;

    cap.read(prevFrame);

    cvtColor(prevFrame,prevgGray,COLOR_BGR2GRAY);

    while(1)
    {
        Mat nextFrame,nextGray;

        cap.read(nextFrame);

        // //所有图片处理完后退出程序
        // if(!cap.read(nextFrame));
        // {
        //     break;
        // }
        imshow("视频图像",nextFrame);

        //计算稠密光流
        cvtColor(nextFrame,nextGray,COLOR_BGR2GRAY);
        Mat_<Point2f> flow;//两个方向的运动速度
        calcOpticalFlowFarneback(prevgGray,nextGray,flow,0.5,3,15,3,5,1.2,0);//稠密光流计算函数(前一帧，后一帧，输出图，金字塔层间比例，金字塔层数，均值窗口尺寸，在每层中迭代次数，像素邻域大小，高斯标准差，计算方法标志)

        Mat xV=Mat::zeros(prevFrame.size(),CV_32FC1);//x轴方向速度
        Mat yV=Mat::zeros(prevFrame.size(),CV_32FC1);//y轴方向速度

        //提取两个方向的速度
        for(int row=0;row<flow.rows;row++)
        {
            for(int col=0;col<flow.cols;col++)
            {
                const Point2f& flow_xy=flow.at<Point2f>(row,col);
                xV.at<float>(row,col)=flow_xy.x;
                yV.at<float>(row,col)=flow_xy.y;
            }
        }

        //计算向量角度和幅值
        Mat magnitude,angle;
        cartToPolar(xV,yV,magnitude,angle);//计算向量角和幅值函数(Vx,Vy,整体幅值，夹角)

        //将角度单位转为角度制
        angle=angle*180.0/CV_PI/2.0;

        //把幅值归一化到0-255便于显示结果
        normalize(magnitude,magnitude,0,255,NORM_MINMAX);

        //计算角度和幅度绝对值
        convertScaleAbs(magnitude,magnitude);
        convertScaleAbs(angle,angle);

        //将幅值和角度生成HSV色彩空间图像
        Mat HSV=Mat::zeros(prevFrame.size(),prevFrame.type());
        vector<Mat> result;
        split(HSV,result);
        result[0]=angle;//决定图像颜色
        result[1]=Scalar(255);
        result[2]=magnitude;//决定图像形态

        //将三个通道合成三通道图像
        merge(result,HSV);

        //HSV转RGB
        Mat rgbimg;
        cvtColor(HSV,rgbimg,COLOR_HSV2RGB);

        //显示结果
        imshow("运动检测结果",rgbimg);

        int t=0;

        int ch=waitKey(5);
        if(ch==27)
        {
            break;
        }
        waitKey(0);
        
        cout<<t<<endl;
        t++;
    }


}

vector<Scalar> color_lut;//颜色查找表

void draw_lines(Mat &image,vector<Point2f> pt1,vector<Point2f> pt2)
{
    RNG rng(5000);
    if(color_lut.size()<pt1.size());
    {
        for(size_t t=0;t<pt1.size();t++)
        {
            color_lut.push_back(Scalar(rng.uniform(0,255),rng.uniform(0,255),rng.uniform(0,255)));

        }

    }
    for(size_t t=0;t<pt1.size();t++)
    {
        line(image,pt1[t],pt2[t],color_lut[t],2,8,0);
    }

}

void xishuguangliu()
{
    VideoCapture cap(0);

    Mat prevframe,prevImg;
    cap.read(prevframe);
    cvtColor(prevframe,prevImg,COLOR_BGR2GRAY);

    //角点检测相关参数设置
    vector<Point2f> Points;
    double qualityleve=0.01;
    int minD=10;
    int blocksize=3;
    bool uhd=false;
    int corners=5000;
    double k=0.04;

    //角点检测
    goodFeaturesToTrack(prevImg,Points,corners,qualityleve,minD,Mat(),blocksize,uhd,k);

    //稀疏光流法相关参数设置
    vector<Point2f> prevpts;
    vector<Point2f> nextpts;
    vector<uchar> status;
    vector<float> err;
    TermCriteria crit=TermCriteria(TermCriteria::COUNT+TermCriteria::EPS,30,0.01);
    double dervl=0.5;
    int flags=0;

    //初始状态角点
    vector<Point2f> initPoints;
    initPoints.insert(initPoints.end(),Points.begin(),Points.end());

    //前一帧角点坐标
    prevpts.insert(prevpts.end(),Points.begin(),Points.end());

    while (1)
    {
        Mat nextframe,nextimg;
        cap.read(nextframe);
        imshow("nextframe",nextframe);

        //光流跟踪
        cvtColor(nextframe,nextimg,COLOR_BGR2GRAY);
        calcOpticalFlowPyrLK(prevImg,nextimg,prevpts,nextpts,status,err,Size(31,31),3,crit,dervl,flags);
        //稀疏光流跟踪函数(前帧，当前帧，前帧稀疏光流点坐标，当前帧稀疏光流点坐标，输出状态向量，输出误差；向量，每层金字塔的搜索窗口大小，金字塔层数，终止迭代条件，匹配光流点操作标志，相应最小特征值)

        //判断角点是否移动，若不移动就删除
        size_t i,k;
        for(i=k=0;i<nextpts.size();i++)
        {
            //距离与状态测量
            double dist=abs(prevpts[i].x-nextpts[i].x)+abs(prevpts[i].y-nextpts[i].y);
            if(status[i] && dist>2)
            {
                prevpts[k]=prevpts[i];
                initPoints[k]=initPoints[i];
                nextpts[k++]=nextpts[i];
                circle(nextframe,nextpts[i],3,Scalar(0,255,0),-1,8);

            }

        }

        //更新角点数目
        nextpts.resize(k);
        prevpts.resize(k);
        initPoints.resize(k);

        //绘制轨迹
        draw_lines(nextframe,initPoints,nextpts);
        imshow("result",nextframe);

        char c=waitKey(50);
        if(c==27)
        {
            break;
        }

        //更性角点坐标和前一帧图片
        std::swap(nextpts,prevpts);
        nextimg.copyTo(prevImg);

        //如果角点数少于30，重新检测角点
        if(initPoints.size()<30)
        {
            goodFeaturesToTrack(prevImg,Points,corners,qualityleve,minD,Mat(),blocksize,uhd,k);
            initPoints.insert(initPoints.end(),Points.begin(),Points.end());
            prevpts.insert(prevpts.end(),Points.begin(),Points.end());

        }


    }
    

}

